//
// Created by chiro on 2021/5/10.
//

#ifndef RADAR_RMCAMERA_CONFIG_H
#define RADAR_RMCAMERA_CONFIG_H

#include <cstdint>
#include <opencv2/opencv.hpp>

namespace rm {
namespace config {
class camera {
public:
  double fx = 811.72293646509797,
    fy = 812.08878700531454,
    cx = 303.92727183847512,
    cy = 307.00937071223274,
    k1 = -0.23008069888906263,
    k2 = 0.53017452547602162,
    p1 = -0.0044045373220139242,
    p2 = -0.0026143012397772166,
    k3 = -1.2392524907626943;
  double FOCUS_PIXEL;
  int64_t roi_width = 1280;
  int64_t roi_height = 1024;
  int64_t roi_offset_x = 0;
  int64_t roi_offset_y = 0;

  cv::Mat mtx, dist, mapx, mapy;

  cv::Mat getCameraMatrix() const {
    return mtx;
  }

  cv::Mat getDistCoeff() const {
    return dist;
  }

  void init() {
    mtx = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    dist = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);
    cv::initUndistortRectifyMap(mtx, dist, cv::noArray(), mtx,
                                cv::Size(roi_width, roi_height), CV_32FC1, mapx, mapy);
    FOCUS_PIXEL = (fx + fy) / 2;
  }

  void undistort(cv::Mat &frame) const {
    remap(frame, frame, mapx, mapy, cv::INTER_LINEAR);
  }
};
};
};

#endif //RADAR_RMCAMERA_CONFIG_H
